.text

.EQU	FIQ_Stack,            0x40001000        /* 512 bytes  */        	
.EQU	IRQ_Stack,            0x40000E00        /* 3584 bytes */        
.EQU	SVC_Stack,            0x30700000        /* 256K bytes */
.EQU	ABT_Stack,            0x30700000        /* 32K bytes */
.EQU	UND_Stack,            0x30700000        /* 32K bytes */
.EQU	USR_Stack,            0x30800000        /* 512K bytes */     
            
											  
	.extern  FIQ_Handler
	.global  FIQ_Handler
	.extern  IRQ_Handler
	.global  IRQ_Handler
	.extern  SWI_Handler
	.global  SWI_Handler
	.extern  Undefined_Handler
	.global  Undefined_Handler
	.extern  DataAbort_Handler
	.global  DataAbort_Handler
	.extern  PrefetchAbort_Handler
	.global  PrefetchAbort_Handler
	.extern  Get_Addr
	.global  Get_Addr 
	
	.extern  SWIHandler
	.extern  UndefinedHandler
	.extern  DataAbortHandler
	.extern  PrefetchAbortHandler
	.extern  FiqHandler
     .extern  Main
	.global  MMU_Init 
	.extern  MMU_Init
    

   .global  __main
__main:

__entry:
        B       Reset_Handler
        B       Undefined_Handler
        B       SWI_Handler
        B       PrefetchAbort_Handler
        B       DataAbort_Handler
        NOP
        B       IRQ_Handler
        B       FIQ_Handler

Undefined_Handler:
        B       UndefinedHandler
PrefetchAbort_Handler:
        B       PrefetchAbortHandler         
DataAbort_Handler:
        B       DataAbortHandler

JUMP_Handler:
		BX      r3
		
FIQ_Handler:
		SUB     lr, lr, #4		
		STMFD   sp!, {r0-r3, lr} 
		LDR     r1, =FIQ_HANDLER
		LDR     r3, [r1,#0x00]
		BL      JUMP_Handler
        	LDMFD   sp!, {r0-r3, pc}^
                   
SWI_Handler:
        STMFD   sp!, {r0-r12,lr} 
		MOV     r1,sp
		MRS     r0,spsr
		STMFD   sp!,{r0}
        
		MRS     r1,cpsr
		BIC     r1,r1,#0xc0
		MSR     cpsr,r1


		LDR     r0,[lr,#-4]
		BIC     r0,r0,#0xFF000000
        
        BL      SWIHandler                      
        
		LDMFD   sp!,{r0}
		MSR     spsr_cf,r0
		
		LDMFD   sp!, {r0-r12,pc}^
           
IRQ_Handler:
        SUB     lr, lr, #4	
		STMFD   sp!, {r0-r3, r12, lr} 	
        MOV     r0, #0x4A000000     
        LDR     r1, [r0, #0x14] 
	LDR     r2, =IRQ_HANDLER_TABLE
        LDR     r3, [r2, r1, lsl #2] 
        BL      JUMP_Handler      				
        LDMFD   sp!, {r0-r3, r12, pc}^       			


.EQU R1_C,       (1<<2)
.EQU R1_I,       (1<<12)
.EQU R1_M,       (1)
.EQU R1_A,       (1<<1)

.global MMU_EnableDCache
.extern MMU_EnableDCache 
MMU_EnableDCache:        
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_C
   mcr  p15,0,r0,c1,c0,0
   MOV  pc,lr

.global MMU_DisableDCache
.extern MMU_DisableDCache 
MMU_DisableDCache:       
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_C
   mcr  p15,0,r0,c1,c0,0
   MOV  pc,lr

.global  MMU_CleanInvalidateDCacheIndex
.extern  MMU_CleanInvalidateDCacheIndex
MMU_CleanInvalidateDCacheIndex:  
   mcr  p15,0,r0,c7,c14,2
   MOV  pc, lr

.global MMU_InvalidateDCache 
.extern MMU_InvalidateDCache 
MMU_InvalidateDCache:
   mcr  p15,0,r0,c7,c6,0
   MOV  pc, lr

.global MMU_EnableICache 
.extern MMU_EnableICache 
MMU_EnableICache:        
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_I
   mcr  p15,0,r0,c1,c0,0
   MOV  pc, lr

.global  MMU_DisableICache
.extern  MMU_DisableICache
MMU_DisableICache:       
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_I
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

.global  MMU_InvalidateICache
.extern  MMU_InvalidateICache
MMU_InvalidateICache:
   mcr  p15,0,r0,c7,c5,0
   mov  pc,lr

.global  MMU_SetDomain
.extern  MMU_SetDomain
MMU_SetDomain:
   mcr  p15,0,r0,c3,c0,0
   MOV pc, lr

.global  MMU_SetTTBase
.extern  MMU_SetTTBase
MMU_SetTTBase:
   mcr  p15,0,r0,c2,c0,0
   mov  pc,lr

.global  MMU_SetAsyncBusMode
.extern  MMU_SetAsyncBusMode
MMU_SetAsyncBusMode:
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#0xC0000000
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr


.global  MMU_EnableMMU
.extern  MMU_EnableMMU
MMU_EnableMMU:
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_M
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

.global  MMU_DisableMMU
.extern  MMU_DisableMMU
MMU_DisableMMU:
   mrc  p15,0,r0,c1,c0,0
   bic  r0,r0,#R1_M
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr
   
.global  MMU_EnableAlignFault 
.extern  MMU_EnableAlignFault
MMU_EnableAlignFault:
   mrc  p15,0,r0,c1,c0,0
   orr  r0,r0,#R1_A
   mcr  p15,0,r0,c1,c0,0
   mov  pc,lr

.global  MMU_InvalidateTLB
.extern  MMU_InvalidateTLB
MMU_InvalidateTLB:      
   mcr  p15,0,r0,c8,c7,0
   MOV  pc,lr
   
.global  MMU_SetProcessId
.extern  MMU_SetProcessId   
MMU_SetProcessId:        
   mcr  p15,0,r0,c13,c0,0
   MOV  pc,lr


MMU_Init:
        STMFD    sp!,{r4-r6,lr}
        BL       MMU_DisableDCache
        BL       MMU_DisableICache
        MOV      r5,#0
Ladd16:
        MOV      r4,#0
        MOV      r6,r5,LSL #26
Ladd24:
        ORR      r0,r6,r4,LSL #5
        BL       MMU_CleanInvalidateDCacheIndex
        ADD      r4,r4,#1
        CMP      r4,#8
        BCC      Ladd24
        ADD      r5,r5,#1
        CMP      r5,#0x40
        BCC      Ladd16
        BL       MMU_InvalidateICache
        BL       MMU_DisableMMU
        BL       MMU_InvalidateTLB
        
        LDR      r0,Ladd424
        MOV      r1,#0
        MOV      r3,#0
        MOV      r2,r0
Ladd84:
        ADD      r1,r1,#1
        CMP      r1,#0x1000
        STR      r3,[r2],#4
        BCC      Ladd84
        
        LDR      lr,Ladd428
        MOV      r2,r0
        MOV      r1,#0
        ADD      r12,lr,#0x800
        B        Ladd140
Ladd120:
        CMP      r1,#2
        BCC      Ladd140
        CMP      r1,#5
        ADDLS    r3,r12,r1,LSL #20
        BLS      Ladd144
Ladd140:
        ADD      r3,lr,r1,LSL #20
Ladd144:
        ADD      r1,r1,#1
        CMP      r1,#5
        STR      r3,[r2],#4
        BCC      Ladd120                
               
        LDR      r1,Ladd432
        LDR      r2,Ladd436
        LDR      r3,Ladd444
        STR      r2,[r1],#0x20         /* set lcd command address */
        
        ADD      r2,r2,#0x800000
        STR      r2,[r1,#0]           /*  set lcd data address */

        LDR      r1,Ladd440
        LDR      r2,Ladd444
        STR      r2,[r1,#0]         /* set extern port1  address */
 
        LDR      r1,Ladd441
        LDR      r2,Ladd445
        STR      r2,[r1,#0]         /* set extern port2  address */

             
        LDR      r2,Ladd448
        LDR      r12,Ladd452
        MOV      r1,#0
Ladd224:
        ADD      r3,r12,r1,LSL #20
        ADD      r1,r1,#1
        CMP      r1,#4
        STR      r3,[r2],#4
        BCC      Ladd224
        
        
         /* add sdram 6,7,8M space    */
        LDR      r1,Ladd50          /*r1=0x30100C14*/
        LDR      r2,Ladd54          /*r2=0x3050040E*/
        STR      r2,[r1],#4         /*set SDRAM 6M space ap=01*/
        #ADD      r2,r2,#0x100000
        LDR      r2,Ladd55
        STR      r2,[r1],#4         /*set SDRAM 7M space ap=01*/
        #ADD      r2,r2,#0x100800
        LDR      r2,Ladd56
        STR      r2,[r1,#0]         /*set SDRAM 8M space ap=11*/
        
        LDR      r1,Ladd456
        LDR      r2,Ladd460
        LDR      r12,Ladd464
        STR      r2,[r1,#0]
        ADD      r2,r1,#0x200
        MOV      r1,#0
Ladd268:
        ADD      r3,r12,r1,LSL #20
        ADD      r1,r1,#1
        CMP      r1,#0x180
        STR      r3,[r2],#4
        BCC      Ladd268
        BL       MMU_SetTTBase
        LDR      r0,Ladd468
        BL       MMU_SetDomain
        MOV      r0,#0
        BL       MMU_SetProcessId
        BL       MMU_EnableAlignFault
        BL       MMU_SetAsyncBusMode
        BL       MMU_EnableMMU
        BL       MMU_EnableICache
        LDMFD    sp!,{r4-r6,lr}
        B        MMU_EnableDCache
 
Ladd424:
	.long    	0x30100000
Ladd428:
	.long    	0x3000040e
	
Ladd432:
	.long    	0x30100400	
Ladd436:
	.long    	0x10000402
	
Ladd440:
	.long    	0x30100600
Ladd444:
	.long    	0x18000402
	
Ladd441:
	.long    	0x30100800
Ladd445:
	.long    	0x20000402
	
Ladd442:
	.long    	0x30100a00
Ladd446:
	.long    	0x28000402
	
Ladd448:
	.long    	0x30100c00
Ladd452:
	.long    	0x00000402
Ladd456:
	.long    	0x30101000
Ladd460:
	.long    	0x40000c02
Ladd464:
	.long    	0x48000402
Ladd468:
	.long    	0x55555555

Ladd50:
    .long       0x30100C14
Ladd54:
    .long       0x3050040e 
Ladd55:
    .long       0x3060040e  
Ladd56:
    .long       0x30700c0e  
    

        
Get_Addr:
		MOV     pc, lr
 
Reset_Handler:
		MOV     R0, #0xdf
        MSR     CPSR_c, R0
        LDR     R13, =FIQ_Stack             
        Bl      Get_Addr                        
        CMP     r14,#0x40000000
        BLS     Next_CMP
        BL      MMU_Init
        MOV     r0, #0x00
        MOV     pc, r0        
Next_CMP:        
        CMP     r14,#0x30000000
        BLS     End_CMP
        MOV      r2,#0x30000000
        MOV      r1,#0x40000000
        MOV      r0,#0
CP_Loop:
        LDR      r3,[r2],#4
        ADD      r0,r0,#1
        CMP      r0,#0x400
        STR      r3,[r1],#4
        BCC      CP_Loop                
        MOV      r0,#0x40000000
        MOV      pc,r0   
        
            
End_CMP:
        MOV     R0, #0xd1
        MSR     CPSR_c, R0
        LDR     R13, =FIQ_Stack     
        MOV     R0, #0xd2
        MSR     CPSR_c, R0
        LDR     R13, =IRQ_Stack 			           			
        MOV     R0, #0xd3
        MSR     CPSR_c, R0
        LDR     R13, =SVC_Stack 			
        MOV     R0, #0xd7
        MSR     CPSR_c, R0
        LDR     R13, =ABT_Stack 			
        MOV     R0, #0xdb
        MSR     CPSR_c, R0
        LDR     R13, =UND_Stack			
        MOV     R0, #0xdf
        MSR     CPSR_c, R0
        LDR     R13, =USR_Stack          

        .extern      Image_RO_Limit      	/* End of ROM code (=start of ROM data) */
        .extern      Image_RW_Base       	/* Base of RAM to initialise */
        .extern      Image_ZI_Base       	/* Base and limit of area */
        .extern      Image_ZI_Limit      	/* to zero initialise */

        ldr         r0, =Image_RO_Limit 	/* Get pointer to ROM data */
        ldr         r1, =Image_RW_Base  	/* and RAM copy */
        ldr         r3, =Image_ZI_Base  	/* Zero init base => top of initialised data */
        cmp         r0, r1                  /* Check that they are different */
        beq         NoRW
LoopRw: CMP     	r1, r3  				/* copy init data */
        LDRCC   	r2, [r0], #4
        STRCC   	r2, [r1], #4
        bcc     	LoopRw
NoRW:   LDR     	r1, =Image_ZI_Limit /* top of zero init segment */
        MOV     	r2, #0
LoopZI: CMP     	r3, r1  				/* zero init */
        STRCC   	r2, [r3], #4
        beq     	LoopZI    
        			           
        MOV     R0, #0x1f
        MSR     CPSR_c, R0
        BL      Main     

.global  dmult
.extern  dmult   
dmult:
	    STMFD   sp!, {r4-r5} 	
		UMULL   r4,r5,r0,r1
        STR     r5,[r2,#0]
        STR     r4,[r3,#0]
        LDMFD   sp!, {r4-r5}      			
        MOV     pc,lr


.global RunStartAddr
.extern RunStartAddr
RunStartAddr:
        STMFD   sp!, {r0-r12,lr}         
        MRS     r1, CPSR           
        BIC     r1, r1, #0x1F           
        ORR     r1, r1, #0x10   
        MSR     CPSR_c, r1 
	    
	    MOV     lr,pc 
		MOV     pc,r0
        LDMFD   sp!, {r0-r12,pc}^  
         			
.global RunAppStartAddr
.extern RunAppStartAddr
RunAppStartAddr:
        STMFD   sp!, {r0-r1,lr}          
	    MOV     lr,pc 
		MOV     pc,r0	
        LDMFD   sp!, {r0-r1,pc} 


.global ClearIDCache
.extern ClearIDCache
ClearIDCache:
        STMFD    sp!,{r0-r12,lr}
        BL       MMU_DisableDCache
        BL       MMU_DisableICache
        BL       MMU_InvalidateICache
        MOV      r5,#0
loop_0:
        MOV      r4,#0
        MOV      r6,r5,LSL #26
loop_1:
        ORR      r0,r6,r4,LSL #5
        BL       MMU_CleanInvalidateDCacheIndex
        ADD      r4,r4,#1
        CMP      r4,#8
        BCC      loop_1
        ADD      r5,r5,#1
        CMP      r5,#0x40
        BCC      loop_0
        BL       MMU_EnableICache
        BL       MMU_EnableDCache  
        LDMFD    sp!, {r0-r12,pc} 
        
.global ClearIDCache1
.extern ClearIDCache1
ClearIDCache1: 
   mrc   p15,0,r0,c1,c0,0
   bic   r0,r0,#R1_I
   mcr   p15,0,r0,c1,c0,0
   
   mcr   p15,0,r0,c7,c5,0
        
   mrc   p15,0,r0,c1,c0,0
   orr   r0,r0,#R1_I
   mcr   p15,0,r0,c1,c0,0
   MOV   pc, lr           
